from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, TimerAction, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
import os
from launch.actions import ExecuteProcess


def generate_launch_description():
    package_path = get_package_share_directory("scepter_moveit_config")

    moveit_config = (MoveItConfigsBuilder("so_arm", package_name="scepter_moveit_config")
                     .robot_description('config/gz.so_arm.urdf.xacro')
                     .robot_description_semantic('config/so_arm.srdf')
                     .to_moveit_configs())

    # 启动Gazebo
    gazebo_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([get_package_share_directory('ros_gz_sim') + '/launch/gz_sim.launch.py']),
        launch_arguments=[('gz_args', 'empty.sdf -r --physics-engine gz-physics-bullet-featherstone-plugin')]
    )

    # 将机械臂添加到Gazebo
    robot_to_gazebo_node = Node(
        package='ros_gz_sim',
        executable='create',
        arguments=[
            '-string',
            moveit_config.robot_description['robot_description'],
            '-x', '0.0', '-y', '0.0', '-z', '0.0', '-name', 'arm'
        ]
    )

    # Clock Bridge
    clock_bridge_node = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
        output='screen'
    )

    # 发布机器人各关节的状态信息
    # 根据机器人的URDF描述和关节状态，计算并发布机器人各连杆的变换关系(TF)
    # 为RViz、MoveIt等工具提供机器人当前状态的可视化数据
    # 设置使用仿真时间(use_sim_time)
    # 以同步Gazebo仿真时间
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            {'use_sim_time': True},
            {"publish_frequency": 30.0}
        ]
    )

    # 发布静态坐标变换
    # 建立world坐标系到机器人base_link坐标系的固定变换关系
    # 这对于将机器人固定在仿真环境中的特定位置非常重要
    static_tf_node = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_s"],
    )

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        output="log",
        arguments=["-d", package_path + '/config/moveit.rviz'],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.planning_pipelines,
            {'use_sim_time': True},
        ]
    )

    # Load controllers
    load_controllers = []
    for controller in [
        "arm_controller",
        "hand_controller",
        "joint_state_broadcaster",
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner {}".format(controller)],
                shell=True,
                output="screen",
            )
        ]

    # 提供高层次的运动规划接口
    # 整合各种规划算法、运动学求解器、碰撞检测等
    # 为用户应用提供规划服务，如路径规划、姿态规划等
    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            moveit_config.to_dict(),
            {'use_sim_time': True}
        ]
    )
    return LaunchDescription(
        [
            robot_state_publisher,
            static_tf_node,
            rviz_node,
            move_group_node,
            TimerAction(period=3.0, actions=[gazebo_node]),
            TimerAction(period=4.0, actions=[robot_to_gazebo_node]),
            TimerAction(period=5.0, actions=[clock_bridge_node]),
        ]
        + load_controllers
    )
